#include "../Device/compass.h"

#include "ahrs.h"
#include "cmsis_os2.h"
#include "main.h"

#define DEVIATION_COMPASS_X (150)
#define DEVIATION_COMPASS_Y (-250)
#define DEVIATION_COMPASS_Z (-600)

extern I2C_HandleTypeDef hi2c1;
const uint8_t t1 = 0x01, t2 = 0x40, t3 = 0x01, t4 = 0x0d;
uint8_t compass_tmp[6];
extern int16_t compass_data_x, compass_data_y, compass_data_z;
extern float Yaw, yaw_setpoint;
uint8_t compass_ready;

void Compass_Init() {
  HAL_I2C_Mem_Write(&hi2c1, 0x1a, 0x0B, I2C_MEMADD_SIZE_8BIT, (uint8_t *)&t1,
                    sizeof(uint8_t), 100);
  HAL_I2C_Mem_Write(&hi2c1, 0x1a, 0x20, I2C_MEMADD_SIZE_8BIT, (uint8_t *)&t2,
                    sizeof(uint8_t), 100);
  HAL_I2C_Mem_Write(&hi2c1, 0x1a, 0x21, I2C_MEMADD_SIZE_8BIT, (uint8_t *)&t3,
                    sizeof(uint8_t), 100);
  HAL_I2C_Mem_Write(&hi2c1, 0x1a, 0x09, I2C_MEMADD_SIZE_8BIT, (uint8_t *)&t4,
                    sizeof(uint8_t), 100);
  while (Yaw == 0) {
    Compass_GetData();
    AHRS_Prase_Compass();
    AHRS_AnalyYaw();
    osDelay(10);
  }
  for (int num = 0; num < 5; num++) Compass_GetData();
  yaw_setpoint = Yaw;
  compass_ready = 1;
}

void Compass_GetData() {
  HAL_I2C_Mem_Read(&hi2c1, 0x1a, 0x00, I2C_MEMADD_SIZE_8BIT,
                   (uint8_t *)compass_tmp, 6 * sizeof(uint8_t), 5);

  if ((compass_tmp[1] << 8) + compass_tmp[0] > 0x7fff)
    compass_data_x = (compass_tmp[1] << 8) + compass_tmp[0] - 0xffff;
  else
    compass_data_x = (compass_tmp[1] << 8) + compass_tmp[0];
  if ((compass_tmp[3] << 8) + compass_tmp[2] > 0x7fff)
    compass_data_y = (compass_tmp[3] << 8) + compass_tmp[2] - 0xffff;
  else
    compass_data_y = (compass_tmp[3] << 8) + compass_tmp[2];
  if ((compass_tmp[5] << 8) + compass_tmp[4] > 0x7fff)
    compass_data_z = (compass_tmp[5] << 8) + compass_tmp[4] - 0xffff;
  else
    compass_data_z = (compass_tmp[5] << 8) + compass_tmp[4];
}
